#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include "common.h"
#include "robot.h"
#include "motorreport.h"

#include "communicate.h"
#include "odom.h"

/**
  * @brief  motor drive 状态监测函数，按照指定策略发送SDO报文，获取实时信息
  * @param  None
  * @retval None
  */
void MotorDriveMonitor(void)
{
    RobotServoGetPositionNow((void *)(&RobotServo[0]));	//获取行走编码器数据
    usleep(10000);
    RobotServoGetPositionNow((void *)(&RobotServo[1]));	//获取行走编码器数据
}

/**
  * @brief  motor drive 接收指令的解析函数,解析完成后，进行上报
  * @param  None
  * @retval None
  */
void MotorDriveDataReported(Cthread_cantask *task)
{
	uint32_t can_id;
	uint16_t mainindex,subindex;
	DATA32_t value;
    int step_plus_conts;

    can_id = task->id;
    
    if( (can_id > 0x580) && (can_id < 0x700))
    {
        mainindex = (task->data[2] << 8) + task->data[1];
        subindex = task->data[3];
        value.raw[0] =  task->data[4];
        value.raw[1] =  task->data[5];
        value.raw[2] =  task->data[6];
        value.raw[3] =  task->data[7];
        
        if(can_id == 0x581)	{		
            if(mainindex == 0x2104)         //读取keya驱动器编码器绝对值
            {
                
            }
            else if(mainindex == 0x2108){   //编码编码器相对值
                step_plus_conts = value.data32; //保存计数值
                //printf("step: %d", step_plus_conts);
                robot_position_updata(step_plus_conts);
            }
            else if(mainindex == 0x210f){	//读温度

            }
            else if(mainindex == 0x2112){

            }			
        }			
    } 
}